/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 *
 * @author Eric Gever
 */
public class PneumaticDemo {
    
    static boolean pistonout=false,toggled=false;
    static Compressor compress;
    DoubleSolenoid valve1;
    static Solenoid valve2;
    
    static Joystick xbox;
    
    public PneumaticDemo(int digport,int relayport,int sole1,int sole2)
    {
        //This constructor initializes both a Double Solenoid and a Compressor.  Use once
        compress = new Compressor(digport,relayport);
        valve1 = new DoubleSolenoid(sole1,sole2);
        xbox = new Joystick(1);
        compress.start();
    }
    
    public PneumaticDemo(int sole1,int sole2)
    {
        //This Constructor initializes just a double solenoid
        valve1 = new DoubleSolenoid(sole1,sole2);
        xbox = new Joystick(1);
    }
    
    public PneumaticDemo(int sole)
    {
        //This Constructor initializes a single solenoid
       valve2=new Solenoid(sole);
    }
    
    public void togglepiston()
    {
        //Toggles piston based on its current state as saved by the pistonout static boolean
        if(!pistonout)
        {
            valve1.set(DoubleSolenoid.Value.kForward);
            pistonout=true;
        }
        else
        {
            valve1.set(DoubleSolenoid.Value.kReverse);
            pistonout=false;
        }
    }
    
    public void togglesingle()
    {
        //For the single solenoid case, toggles a solenoid on or off
        if(!pistonout)
        {
            valve2.set(true);
            pistonout=true;
        }
        else
        {
            valve2.set(false);
            pistonout=false;
        }
    }
    
    public void disablepiston()
    {
        valve1.set(DoubleSolenoid.Value.kOff);
        valve2.set(false);
        
    }
    
    public void disablecompressor()
    {
        compress.stop();
        compress.free();
    }
    
    public void enablecompressor()
    {
        compress.start();
    }
    
    public void pistonbuttonhold(int outbutton,int offbutton)
    {
        if(xbox.getRawButton(outbutton))
        {
            valve1.set(DoubleSolenoid.Value.kForward);
        }
        else if(xbox.getRawButton(offbutton))
        {
            valve1.set(DoubleSolenoid.Value.kOff);
        }
        else
        {
            valve1.set(DoubleSolenoid.Value.kReverse);
        }
    }
    
    public void pistonmultibutton(int buttonout, int buttonin)
    {
        if(xbox.getRawButton(buttonout))
        {
            valve1.set(DoubleSolenoid.Value.kForward);
        }
        else if(xbox.getRawButton(buttonin))
        {
            valve1.set(DoubleSolenoid.Value.kReverse);
        }
        else
        {
            
        }
    }
    
    public void pistontogglebutton(int button)
    {
        if(xbox.getRawButton(button)&&!toggled)
        {
            toggled=true;
            togglepiston();
        }
        else if(!xbox.getRawButton(button))
        {
            toggled=false;
        }
    }
    
    public void displaystatus()
    {
        try{
        SmartDashboard.putBoolean("Compressor State: ", compress.enabled());
        //SmartDashboard.putBoolean("Switch Value: ", compress.getPressureSwitchValue());
        SmartDashboard.putBoolean("Awesome Button: ", xbox.getRawButton(1));
        SmartDashboard.putBoolean("Button 2: ", xbox.getRawButton(2));
        SmartDashboard.putBoolean("Button 3: ", xbox.getRawButton(3));
        SmartDashboard.putBoolean("Button 4: ", xbox.getRawButton(4));
        SmartDashboard.putBoolean("Button 5: ", xbox.getRawButton(7));
        SmartDashboard.putBoolean("Toggle State: ", toggled);
        
        }
        
        catch(Exception exp)
        {
            //SmartDashboard.putString("Error", "Error outputting Compressor display");
            
        }
    }
    public void displaypistonpos(int pistonID)
    {
        try
        {
            SmartDashboard.putNumber("Piston "+pistonID+" Position: ", valve1.get().value);
        }
        catch(Exception exp)
        {
            SmartDashboard.putString("Error2", "Error outputting Piston ID");
        }
    }
    
}
